/*******************************************************************************************************/
/*******************************************************************************************************/
//   //                                                                         //
//   // $Workfile:: load_sensing.c                                                                            $Revision:: 1                                                                             //
//   // $JustDate:
//   //                                                                         //
//   //      Hy-Brid ECU                                                              //
//   //      Load Sensing algorithm                                                      //
//   //                                                                         //
/*******************************************************************************************************/
/*******************************************************************************************************/
//   //                                                                         //
//   // int load_sense ( unsigned int ui_Calibrated_Scissor_Angle_Array[]                      // Input: The array of stored scissor angles
//   //             , unsigned int ui_Calibrated_Cylinder_Pressure_Array[]                     // Input: The cooresponding array of stored cylinder pressures
//   //             , unsigned int ui_NoLoad_Angle_Sensor_Array[]                           // Input: The array of stored scissor angles
//   //             , unsigned int ui_NoLoad_Pressure_Sensor_Array[]                        // Input: The cooresponding array of stored cylinder pressures
//   //             , unsigned char   uc_Load_Sensing_Array_Size                            // Input: The number of elements in each stored array
//   //             , unsigned int *ui_Measured_Scissor_Angle                            // Output: The measured scissor angle as measure in this function
//   //             , unsigned int *ui_Measured_Cylinder_Pressure)                          // Output: The measured cylinder pressure as measured in this function
//   //                                                                         //
//   // DESCRIPTION: This procedure compares the scissor angle against the                        //
//   //          calculated pressure for that angle.  It then returns the percentage              //
//   //          of the measured pressure as compared to an array of stored pressures.            //
//   //                                                                         //
//   //                                                                         //
/*******************************************************************************************************/
/*******************************************************************************************************/
 /* */																									//
 /* */	#include "load_sensing.h"																		//
 /* */	#include "main.h"																				//
 /* */	#include "common.h"																				//
 /* */	#include "calibrate_load_sensing.h"																//
 /* */	#include "fet.h"																				//
#include "nvm.h"
#include "control.h"
#include "switches.h"

//    static long wFudgeValue; // ET_WORKING <FUDGE>

/* */                                                                           //
/*******************************************************************************************************/
/*******************************************************************************************************/

int load_sense(unsigned int   ui_Calibrated_Scissor_Angle_Array[]                        // The array of stored scissor angles
               , unsigned int  ui_Calibrated_Cylinder_Pressure_Array[]                    // The cooresponding array of stored cylinder pressures
               , unsigned int  ui_NoLoad_Angle_Sensor_Array[]                          // The array of stored scissor angles
               , unsigned int  ui_NoLoad_Pressure_Sensor_Array[]                       // The cooresponding array of stored cylinder pressures
               , unsigned int  ui_Up_Calibrated_Scissor_Angle_Array[]                     // The array of stored scissor angles
               , unsigned int  ui_Up_Calibrated_Cylinder_Pressure_Array[]                    // The cooresponding array of stored cylinder pressures
               , unsigned int  ui_Up_NoLoad_Angle_Sensor_Array[]                          // The array of stored scissor angles
               , unsigned int  ui_Up_NoLoad_Pressure_Sensor_Array[]                       // The cooresponding array of stored cylinder pressures
               , unsigned int  ui_Dn_Calibrated_Scissor_Angle_Array[]                     // The array of stored scissor angles
               , unsigned int  ui_Dn_Calibrated_Cylinder_Pressure_Array[]                    // The cooresponding array of stored cylinder pressures
               , unsigned int  ui_Dn_NoLoad_Angle_Sensor_Array[]                          // The array of stored scissor angles
               , unsigned int  ui_Dn_NoLoad_Pressure_Sensor_Array[]                       // The cooresponding array of stored cylinder pressures
               , unsigned char uc_Load_Sensing_Array_Size                           // The number of elements in each stored array
               , unsigned int  *ui_Measured_Scissor_Angle                           // The measured scissor angle as measure in this function
               , unsigned int  *ui_Measured_Cylinder_Pressure                          // The measured cylinder pressure as measured in this function
               , unsigned int   *ui_Measured_Cylinder_Pressure2)

{
   int  i_Pressure_Percent_of_Maximum;
   unsigned int   ui_Interpolated_Max_Cylinder_Pressure;
   unsigned int   ui_Interpolated_Min_Cylinder_Pressure;
   static long    sl_Filtered_Pressure_Percent_of_Maximum;
   static unsigned int ui_Measured_Cylinder_Pressure_Last;
   static unsigned char ub_up_last = 0;

   *ui_Measured_Cylinder_Pressure = read_cylinder_pressure();                         // Read the cylinder pressure and scissor angle
   *ui_Measured_Scissor_Angle = read_angle_sensor();                                // These reads occur every 10 mSec (The backgroung time).
   *ui_Measured_Cylinder_Pressure2 = read_cylinder_pressure2();
   if(Fets.word & FET_UP)
   {
      ub_up_last = 1;
   }
   else if(Fets.word & FET_DOWN)
   {
      ub_up_last = 0;
   }

   if(Fets.word & FET_UP)
   {
      interpolate_curve(&ui_Up_Calibrated_Scissor_Angle_Array[0]
                        , &ui_Up_Calibrated_Cylinder_Pressure_Array[0]
                        , uc_Load_Sensing_Array_Size
                        , ui_Measured_Scissor_Angle
                        , &ui_Interpolated_Max_Cylinder_Pressure);

      interpolate_curve(&ui_Up_NoLoad_Angle_Sensor_Array[0]
                        , &ui_Up_NoLoad_Pressure_Sensor_Array[0]
                        , uc_Load_Sensing_Array_Size
                        , ui_Measured_Scissor_Angle
                        , &ui_Interpolated_Min_Cylinder_Pressure);

   }
   else if(ub_up_last)
   {
      interpolate_curve(&ui_Calibrated_Scissor_Angle_Array[0]
                        , &ui_Calibrated_Cylinder_Pressure_Array[0]
                        , uc_Load_Sensing_Array_Size
                        , ui_Measured_Scissor_Angle
                        , &ui_Interpolated_Max_Cylinder_Pressure);

      interpolate_curve(&ui_NoLoad_Angle_Sensor_Array[0]
                        , &ui_NoLoad_Pressure_Sensor_Array[0]
                        , uc_Load_Sensing_Array_Size
                        , ui_Measured_Scissor_Angle
                        , &ui_Interpolated_Min_Cylinder_Pressure);
   }
   else
   {
      interpolate_curve(&ui_Dn_Calibrated_Scissor_Angle_Array[0]
                        , &ui_Dn_Calibrated_Cylinder_Pressure_Array[0]
                        , uc_Load_Sensing_Array_Size
                        , ui_Measured_Scissor_Angle
                        , &ui_Interpolated_Max_Cylinder_Pressure);

      interpolate_curve(&ui_Dn_NoLoad_Angle_Sensor_Array[0]
                        , &ui_Dn_NoLoad_Pressure_Sensor_Array[0]
                        , uc_Load_Sensing_Array_Size
                        , ui_Measured_Scissor_Angle
                        , &ui_Interpolated_Min_Cylinder_Pressure);
   }

   if(*ui_Measured_Cylinder_Pressure != 0xFFFF)
   {
      ui_Measured_Cylinder_Pressure_Last = *ui_Measured_Cylinder_Pressure;
   }

   i_Pressure_Percent_of_Maximum =                                            // Calculate the percent of max pressure for use in load-sensing safety algorithms.
                                   (int)((((long)((signed)(ui_Measured_Cylinder_Pressure_Last)                                       // This area is for systems calibrated using both the empty platform and the 100% loaded platform calibration sequences.
                                   - (signed)ui_Interpolated_Min_Cylinder_Pressure) * 100l)
                                   + (long)(((signed)ui_Interpolated_Max_Cylinder_Pressure - (signed)ui_Interpolated_Min_Cylinder_Pressure) / 2))
                                   / (long)((signed)ui_Interpolated_Max_Cylinder_Pressure
                                   - (signed)ui_Interpolated_Min_Cylinder_Pressure));

   i_Pressure_Percent_of_Maximum = first_order_low_pass_filter                         // Filter the result.
                                   (&sl_Filtered_Pressure_Percent_of_Maximum
                                    , PRESSURE_SENSING_TIME_CONSTANT
                                    , i_Pressure_Percent_of_Maximum);
   return i_Pressure_Percent_of_Maximum;                                         // This can be negative, due to drifting pressure sensor or angle sensor readings.

}
/**********END**********END**********END**********END**********END**********END**********END************/



/***************************************************************************************/
/***************************************************************************************/
//                                                                  //
//   A function to interpolate avalue off of an array from another value.          //
//                                                                  //
//                                                                  //
/***************************************************************************************/
/***************************************************************************************/

unsigned int interpolate_curve(unsigned int ui_Array1[]
                               , unsigned int ui_Array2[]
                               , unsigned char uc_array_size
                               , unsigned int *Array1_value
                               , unsigned int *Array2_value)
{
   unsigned char uc_ArrayPosition;
   // unsigned int ui_Interpolated_value;
   unsigned int ui_Percent_Interpolation;

   for(uc_ArrayPosition = 0; uc_ArrayPosition < uc_array_size; uc_ArrayPosition++)           // Calculate the maximum fully-loaded cylinder pressure.
   {  // This must work for both increasing and decreasing angle sensing elements.
      if(((ui_Array1[uc_array_size - 1] >  ui_Array1[0])                         // Increasing voltage with increasing angle.
          && (*Array1_value >= ui_Array1[uc_array_size - 1])) // Platform is too high.
         || ((ui_Array1[uc_array_size - 1] <   ui_Array1[0])                     // Decreasing voltage with increasing angle.
             && (*Array1_value <= ui_Array1[uc_array_size - 1]))) // Platform is too high.
      {  //
         *Array2_value = ui_Array2[uc_array_size - 1]; //
         uc_ArrayPosition = uc_array_size;                        //  Only enter this loop once.  Then exit the 'for' loop.
      }                                                              //
      else if(((ui_Array1[uc_array_size - 1] > ui_Array1[0])                     // Increasing voltage with increasing angle.
               && (*Array1_value < ui_Array1[uc_ArrayPosition]))           // The measured angle is approaching tne next calibrated value.
              || ((ui_Array1[uc_array_size - 1] < ui_Array1[0])                    // Decreasing voltage with increasing angle.
                  && (*Array1_value > ui_Array1[uc_ArrayPosition])))          // The measured angle is approaching tne next calibrated value.
      {  //
         if(uc_ArrayPosition == 0)                                         // The Lift is still in the full-down position.  The angle is less than the calibrated down angle.
         {  //
            *Array2_value =   0x03FE;        // Trip at the full-down pressure value.
         }                                                           //
         else                                                        //
         {  // Determine the location between calibration points and use this value to interpolate the theoretical pressure.
            ui_Percent_Interpolation = (unsigned int)                                  //
                                       ((((long) * Array1_value                  //
                                          - (long)ui_Array1[uc_ArrayPosition - 1]) * 100l) //
                                        / ((long)ui_Array1[uc_ArrayPosition]   //
                                           - (long)ui_Array1[uc_ArrayPosition - 1])); //
            //
            *Array2_value = (unsigned int)                           // Use the interpolation value to determine the theoretical intermediate pressure.
                            ((long)ui_Array2[uc_ArrayPosition - 1] //
                             + ((((long)ui_Array2[uc_ArrayPosition]//
                                  - (long)ui_Array2[uc_ArrayPosition - 1])//
                                 * (long)ui_Percent_Interpolation) / 100l));              //
         }                                                           //
         uc_ArrayPosition = uc_array_size;                           // Only enter this loop once.  Then exit the 'for' loop.
      }                                                              //
   }                                                                 //
   return ui_Percent_Interpolation;
}
/*********END**********END**********END**********END**********END**********END**********/



/*******************************************************************************************************/
/*******************************************************************************************************/
//                                                                         //
// A function to read the cylinder pressure.                                        //
//                                                                         //
//                                                                         //
/*******************************************************************************************************/
/*******************************************************************************************************/
unsigned int read_cylinder_pressure(void)                                        //
{  //
   unsigned int ui_Measured_Cylinder_Pressure;                                      //
   static long sl_Filtered_Pressure_Measurement1 = 0;                               //
   // static long sl_Filtered_Pressure_Measurement2 = 0;                               //
   unsigned int ui_Filtered_Cylinder_Pressure1;                                  //
   // unsigned int ui_Filtered_Cylinder_Pressure2;                                  //
   // unsigned int ui_Pressure_Difference;                                          //
   int i_Time_Constant1;                                         //
   //
   i_Time_Constant1 = CYLINDER_PRESSURE_TIME_CONSTANT;
   // A/D Conversion on one channel only.
   ui_Measured_Cylinder_Pressure = u16GetADCResult(PRESSURE_SENSOR_AD_CHANNEL_1);// 10-Bit A/D conversion. Data is read from bits 15 through 6.
   // Bits 0 - 5 are read as                                                                 +
   ui_Filtered_Cylinder_Pressure1 =                                              //
      first_order_low_pass_filter(&sl_Filtered_Pressure_Measurement1           // Filter the measurement.
                                  , i_Time_Constant1         //
                                  , ui_Measured_Cylinder_Pressure);         //
   //
   return ui_Filtered_Cylinder_Pressure1;

   //#if( 0 )
   // AD.CSR.BIT.ADF =  0;                                                       // Clear the conversion complete status flag.
   // AD.CSR.BIT.SCAN =  0;                                                      // A/D Conversion on one channel only.
   // AD.CSR.BIT.CH = PRESSURE_SENSOR_AD_CHANNEL_2;                                    // Select the Pressure Sensor channel.
   // AD.CSR.BIT.CKS    = 1;                                                     // High speed conversion.
   // AD.CSR.BIT.ADST   = 1;                                                     // START conversion.  Each conversion is
   // while ( 0 == AD.CSR.BIT.ADF)                                               // 4.75 uSec when SysClk = 14.7456 MHz.
   // ;                                                                       // Wait until not busy.
   // ui_Measured_Cylinder_Pressure = AD.DRC;                                       // Store the conversion from A/D channel 2.
   // ui_Measured_Cylinder_Pressure >>= 6;                                          // 10-Bit A/D conversion. Data is read from bits 15 through 6.
   // ui_Measured_Cylinder_Pressure &= 0x3FF;                                          // 10-Bit A/D conversion. Data is read from bits 15 through 6.
   //                                                                      // Bits 0 - 5 are read as 0.
   ////     if (lift_options.fOverload2_cutout)
   ////     {
   //       ui_Filtered_Cylinder_Pressure2 =                                              //
   //                first_order_low_pass_filter( &sl_Filtered_Pressure_Measurement2            // Filter the measurement.
   //                                     ,i_Time_Constant1          //
   //                                     ,ui_Measured_Cylinder_Pressure);
   ////     }
   ////     else
   ////     {
   ////        ui_Filtered_Cylinder_Pressure2 = ui_Filtered_Cylinder_Pressure1;
   ////     }

   //    if (ui_Filtered_Cylinder_Pressure1 > ui_Filtered_Cylinder_Pressure2)                // Find the absoulte value of the difference between the two pressure measurements.
   //        ui_Pressure_Difference = ui_Filtered_Cylinder_Pressure1 - ui_Filtered_Cylinder_Pressure2;   //
   //    else                                                                 //
   //        ui_Pressure_Difference = ui_Filtered_Cylinder_Pressure2 - ui_Filtered_Cylinder_Pressure1;   //

   //    if (ui_Filtered_Cylinder_Pressure1 < MINIMUM_ALLOWABLE_PRESSURE_SENSOR_VOLTAGE) return ui_Filtered_Cylinder_Pressure1;
   //    if (ui_Filtered_Cylinder_Pressure2 < MINIMUM_ALLOWABLE_PRESSURE_SENSOR_VOLTAGE) return ui_Filtered_Cylinder_Pressure2;

   ////  if (ui_Pressure_Difference > PRESSURE_DIFFERENCE_THRESHOLD) return 0xFFFF;             // If the sensors do nat show the same pressure, return an error value.
   // if ((ui_Pressure_Difference > PRESSURE_DIFFERENCE_THRESHOLD) || ((MAIN_Data.wTimeInState % 200) == 0))
   //    return 0xFFFF;             // If the sensors do nat show the same pressure, return an error value.
   //    else
   //        return ui_Filtered_Cylinder_Pressure1;                                       // Else return the value from one of the sensors.
   //#endif
}

unsigned int read_cylinder_pressure2(void)                                          //
{  //
   unsigned int ui_Measured_Cylinder_Pressure;                                      //
   static long sl_Filtered_Pressure_Measurement2 = 0;                               //
   unsigned int ui_Filtered_Cylinder_Pressure2;                                  //                                        //
   int i_Time_Constant1;                                          //
   //
	i_Time_Constant1 = CYLINDER_PRESSURE_TIME_CONSTANT;													//	10-Bit A/D conversion. Data is read from bits 15 through 6.
   ui_Measured_Cylinder_Pressure = u16GetADCResult(PRESSURE_SENSOR_AD_CHANNEL_2);   // 10-Bit A/D conversion. Data is read from bits 15 through 6.
   // Bits 0 - 5 are read as 0.
   ui_Filtered_Cylinder_Pressure2 =                                              //
      first_order_low_pass_filter(&sl_Filtered_Pressure_Measurement2           // Filter the measurement.
                                  , i_Time_Constant1           //
                                  , ui_Measured_Cylinder_Pressure);           //
   //
   return ui_Filtered_Cylinder_Pressure2;
}
/**********END**********END**********END**********END**********END**********END**********END************/





/***********************************************************************************************/
/***********************************************************************************************/
//   //                                                                   //
//   // A function to read the angle sensor.                                       //
//   //                                                                   //
//   //                                                                   //
/***********************************************************************************************/
/***********************************************************************************************/
//
unsigned int read_angle_sensor(void)                                             //
{  //
   unsigned int ui_Measured_Scissor_Angle;                                          //
   static long sl_Filtered_Angle_Measurement = 0;                                   //
   // static unsigned long sul_System_Time = 0;                                     //
   unsigned int ui_Filtered_Scissor_Angle;                                          //
   // unsigned long Temp;                                                                    //

   // A/D Conversion on one channel only.
   ui_Measured_Scissor_Angle = u16GetADCResult(ANGLE_SENSOR_AD_CHANNEL);   // 10-Bit A/D conversion. Data is read from bits 15 through 6.
   // Bits 0 - 5 are read as 0.              //
   ui_Filtered_Scissor_Angle =                                                //
      first_order_low_pass_filter(&sl_Filtered_Angle_Measurement            // Filter the measurement.
                                  , ANGLE_SENSOR_TIME_CONSTANT              //
                                  , ui_Measured_Scissor_Angle);          //
   return ui_Filtered_Scissor_Angle;                                          //
}                                                                       //
//
/*******END**********END**********END**********END**********END**********END**********END*******/



/*******************************************************************************************************/
/*******************************************************************************************************/
//   //                                                                         //
// //   A first-order low-pass filter                                                 //
//   //                                                                         //
// //   The Time constant must be in even multiples of 10 mSec (the same as the background loop time).  //
//   //                                                                         //
//   //                                                                         //
//   //                                                                         //
/*******************************************************************************************************/
/*******************************************************************************************************/
/* */                                                                           //
/* */   int first_order_low_pass_filter(long *l_Old_Filtered_Value                             //
                                        /* */                           , int i_Time_Constant                              //
                                        /* */                           , int i_New_Measurement)                              //
 /* */	{																								//
   /* */      int i_New_Filtered_Value;                                                  //
   /* */                                                                           //
   /* */      *l_Old_Filtered_Value = *l_Old_Filtered_Value                                    //
                                      /* */                        + (((((long)(i_New_Measurement) * 1024l)                       // Multily by 1024
                                            /* */                        - *l_Old_Filtered_Value) * (long)(BACKGROUND_LOOP_TIME))             //
                                            /* */                        / ((long)(BACKGROUND_LOOP_TIME + i_Time_Constant)));           //
   /* */                                                                           //
   /* */      i_New_Filtered_Value = (int)((*l_Old_Filtered_Value + 512l) / 1024l);                     // Round up and divide by 1024
   /* */      return i_New_Filtered_Value;                                               //
   /* */}                                                                       //
/* */                                                                           //
/**********END**********END**********END**********END**********END**********END**********END************/





/*******************************************************************************************************/
/*******************************************************************************************************/
/******Test Code******Remove this section***********************Test Code******Remove this section******/
/* */                                                                           //
/* *///    if (uc_Pressure_Percent_of_Maximum > 100)                                  //
/* *///    {                                                                 //
/* *///       for ( ui_Pressure_Percent_of_Maximum = 0;                               //
/* *///             ui_Pressure_Percent_of_Maximum < 1;                               //
/* *///             ui_Pressure_Percent_of_Maximum++)                                 //
/* *///       {                                                              //
/* *///          Fets.word = TEST_STEER_RIGHT;                                     //
/* *///          delay_mSec(200);                                               //
/* *///          Fets.word = TEST_STEER_LEFT;                                      //
/* *///          delay_mSec(200);                                               //
/* *///       }                                                              //
/* *///    }                                                                 //
/* */                                                                           //
/******Test Code******Remove this section***********************Test Code******Remove this section******/
/*******************************************************************************************************/
/*******************************************************************************************************/





